#include <memory>
#include <thread>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
// 可视化操作
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

int main(int argc, char *argv[])
{
  // Initialize ROS and create the Node
  rclcpp::init(argc, argv);
  auto const node = std::make_shared<rclcpp::Node>(
      "hello_moveit",
      rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));

  // Create a ROS logger
  auto const logger = rclcpp::get_logger("hello_moveit");

  // We spin up a SingleThreadedExecutor so MoveItVisualTools interact with ROS
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  auto spinner = std::thread([&executor]()
                             { executor.spin(); });
  // Create the MoveIt MoveGroup Interface
  using moveit::planning_interface::MoveGroupInterface;
  auto move_group_interface = MoveGroupInterface(node, "panda_arm");

  // -------------------------------------------------------------------------------------
  // 设置速度和加速度因子
  move_group_interface.setMaxVelocityScalingFactor(1.0);
  move_group_interface.setMaxAccelerationScalingFactor(1.0);
  // 设置规划时间
  move_group_interface.setPlanningTime(10.0);
  // 设置规划算法
  move_group_interface.setPlannerId("RRTConnect");
  // -------------------------------------------------------------------------------------

  // -------------------------------------------------------------------------------------
  // Construct and initialize MoveItVisualTools
  auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools{
      node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC,
      move_group_interface.getRobotModel()};
  moveit_visual_tools.deleteAllMarkers();
  moveit_visual_tools.loadRemoteControl();

  // 创建用于可视化的闭包
  auto const draw_title = [&moveit_visual_tools](auto text)
  {
    // 定义一个匿名函数来创建文本的位置
    auto const text_pose = []
    {
      // 创建一个单位变换矩阵（无旋转和平移）
      auto msg = Eigen::Isometry3d::Identity();
      // 设置文本的z轴位置为1.0
      msg.translation().z() = 1.0;
      // 返回创建好的位姿
      return msg;
    }();
    moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE, // 文本颜色为白色
                                    rviz_visual_tools::XLARGE);                // 字体大小为XLARGE
  };
  // 在RViz中显示提示信息
  auto const prompt = [&moveit_visual_tools](auto text)
  {
    // 使用moveit_visual_tools的prompt方法在RViz中显示提示信息
    moveit_visual_tools.prompt(text);
  };
  // 在RViz中绘制工具路径
  auto const draw_trajectory_tool_path =
      [&moveit_visual_tools,                                           // 捕获moveit_visual_tools对象的引用
       jmg = move_group_interface.getRobotModel()->getJointModelGroup( // 获取机器人的关节模型组
           "panda_arm")](auto const trajectory)                        // 捕获关节模型组jmg
  {
    // 使用moveit_visual_tools的publishTrajectoryLine方法在RViz中绘制路径
    moveit_visual_tools.publishTrajectoryLine(trajectory, jmg);
  };
  // -------------------------------------------------------------------------------------

  // // 设置初始关节角度 
  // std::vector<double> joint_positions = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // 7个关节
  // move_group_interface.setJointValueTarget(joint_positions);

  // // 执行到初始位姿
  // moveit::planning_interface::MoveGroupInterface::Plan plan_init;
  // bool init_success = static_cast<bool>(move_group_interface.plan(plan_init));
  // if (init_success)
  // {
  //   move_group_interface.execute(plan_init);
  // }
  // else
  // {
  //   RCLCPP_ERROR(logger, "Failed to plan to initial joint positions.");
  // }

  // -------------------------------------------------------------------------------------
  // 设置目标位姿
  geometry_msgs::msg::Pose target_pose;
  target_pose.orientation.w = 1.0;
  target_pose.position.x = 0.28;
  target_pose.position.y = 0.4;
  target_pose.position.z = 0.5;
  move_group_interface.setPoseTarget(target_pose);
  // -------------------------------------------------------------------------------------

  // -------------------------------------------------------------------------------------
  // 创建一个碰撞对象
  moveit_msgs::msg::CollisionObject collision_object;
  // 获取规划参考坐标系
  std::string frame_id = move_group_interface.getPlanningFrame();
  collision_object.header.frame_id = frame_id; // 设置碰撞对象的参考坐标系
  collision_object.id = "box1";                // 设置碰撞对象的唯一标识符
  // 定义碰撞对象的形状（一个长方体）
  shape_msgs::msg::SolidPrimitive primitive;
  primitive.type = primitive.BOX;              // 设置形状类型为长方体
  primitive.dimensions.resize(3);              // 确保有足够的维度
  primitive.dimensions[primitive.BOX_X] = 0.5; // 设置长方体的X维度
  primitive.dimensions[primitive.BOX_Y] = 0.1; // 设置长方体的Y维度
  primitive.dimensions[primitive.BOX_Z] = 0.5; // 设置长方体的Z维度

  // 定义碰撞对象的位姿（相对于参考坐标系的位置和方向）
  geometry_msgs::msg::Pose box_pose;
  box_pose.orientation.w = 1.0; // 设置四元数的W分量为1，表示无旋转
  box_pose.position.x = 0.2;    // 设置碰撞对象的X位置
  box_pose.position.y = 0.2;    // 设置碰撞对象的Y位置
  box_pose.position.z = 0.25;   // 设置碰撞对象的Z位置

  // 将形状和位姿添加到碰撞对象中
  collision_object.primitives.push_back(primitive);
  collision_object.primitive_poses.push_back(box_pose);
  collision_object.operation = collision_object.ADD; // 设置操作为添加碰撞对象

  // 添加碰撞对象到规划场景中
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  planning_scene_interface.applyCollisionObject(collision_object);
  // -------------------------------------------------------------------------------------

  // 位姿规划
  moveit::planning_interface::MoveGroupInterface::Plan plan;
  bool success = static_cast<bool>(move_group_interface.plan(plan));

  if (!success)
  {
    std::cerr << "Failed to plan to target pose." << std::endl;
  }
  else
  {
    std::cout << "Plan successfully generated." << std::endl;
  }

  // Execute the plan
  if (success)
  {
    // 绘制工具路径
    draw_trajectory_tool_path(plan.trajectory_);
    // 刷新RViz视图，使路径显示出来
    moveit_visual_tools.trigger();
    // 提示用户在RViz中按下'Next'按钮以执行规划
    prompt("Press 'Next' in the RvizVisualToolsGui window to execute");
    // 显示标题“Executing”表示正在执行
    draw_title("Executing");
    // 再次刷新RViz视图
    moveit_visual_tools.trigger();
    // 执行规划
    move_group_interface.execute(plan);
  }
  else
  {
    draw_title("Planning Failed!");
    moveit_visual_tools.trigger();
    RCLCPP_ERROR(logger, "Planing failed!");
  }
  // Shutdown ROS
  rclcpp::shutdown();
  spinner.join(); // <--- Join the thread before exiting
  return 0;
}